﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using Shape;
using AForge;
using System.Drawing.Imaging;
using AForge.Imaging.Filters;
using System.Diagnostics;
using AForge.Imaging;
using AForge.Math.Geometry;

namespace RoadLaneDetection
{
    public static class RoadLane
    {
        public static /*List<Polygon>*/ Bitmap Detect(Bitmap image, double colorValue)
        {
            Debug.WriteLine(">>Starting to process");
            if (colorValue > 1.0 || colorValue < 0.0) throw new ArgumentException("Color value should be in range [0.0..1.0]");
            //List<> lanes = new List<Polygon>();
            Debug.WriteLine("Starting Grayscale...");
            ConvertToGrayscale(image);
            //image = ColorToGrayscale(image);
            BitmapData objectsData = image.LockBits(
                    new Rectangle(0, 0, image.Width, image.Height),
                    ImageLockMode.ReadOnly, image.PixelFormat);

            //Debug.WriteLine("Starting Edge Enhancement...");
            //// create filter with kernel size equal to 11
            //// and Gaussia sigma value equal to 4.0
            //GaussianSharpen edgeFilter = new GaussianSharpen(4, 11);
            //// apply the filter
            //edgeFilter.ApplyInPlace(objectsData);


            #region finding Edge //Commented
            //Debug.WriteLine("Finding edge.");
            //UnmanagedImage edgeUImage = new UnmanagedImage(objectsData);
            //Debug.WriteLine(edgeUImage.PixelFormat.ToString());
            //new DifferenceEdgeDetector().ApplyInPlace(edgeUImage);
            #endregion

            Debug.WriteLine("Stating Color Filter...");
            ColorFiltering colorFilter = new ColorFiltering();
            //set range with (min,max) value
            colorFilter.Blue = new IntRange((int)(colorValue * 255), 255);
            colorFilter.Red = new IntRange((int)(colorValue * 255), 255);
            colorFilter.Green = new IntRange((int)(colorValue * 255), 255);
            colorFilter.ApplyInPlace(objectsData);

            //
            //Debug.WriteLine("Merging edge with color filter image");
            //Merge mergeFileter = new Merge(edgeUImage);
            //mergeFileter.ApplyInPlace(objectsData);
            //

            Debug.WriteLine("Starting Closing Filter...");
            new Closing().ApplyInPlace(objectsData);

            #region Blobing
            Debug.WriteLine("Started Blobing...");
            // locating objects
            BlobCounter blobCounter = new BlobCounter();

            blobCounter.FilterBlobs = true;
            blobCounter.MinHeight = 15;
            blobCounter.MinWidth = 15;

            blobCounter.ProcessImage(objectsData);
            Blob[] blobs = blobCounter.GetObjectsInformation();
            image.UnlockBits(objectsData);

            // check objects' type and highlight
            SimpleShapeChecker shapeChecker = new SimpleShapeChecker();
            using (Graphics g = Graphics.FromImage(image))
            {
                Pen yellowPen = new Pen(Color.Yellow, 2); // circles
                Pen redPen = new Pen(Color.Red, 2);       // quadrilateral
                Pen brownPen = new Pen(Color.Brown, 2);   // quadrilateral with known sub-type
                Pen greenPen = new Pen(Color.Green, 2);   // known triangle
                Pen bluePen = new Pen(Color.Blue, 2);     // triangle

                Debug.WriteLine("Number of blobs found " + blobs.Length.ToString());
                for (int i = 0, n = blobs.Length; i < n; i++)
                {
                    List<IntPoint> edgePoints = blobCounter.GetBlobsEdgePoints(blobs[i]);

                    DoublePoint center;
                    double radius;

                    // is circle ?
                    if (shapeChecker.IsCircle(edgePoints, out center, out radius))
                    {
                        g.DrawEllipse(yellowPen,
                            (float)(center.X - radius), (float)(center.Y - radius),
                            (float)(radius * 2), (float)(radius * 2));
                    }
                    else
                    {
                        List<IntPoint> corners;

                        // is triangle or quadrilateral
                        if (shapeChecker.IsConvexPolygon(edgePoints, out corners))
                        {
                            // get sub-type
                            PolygonSubType subType = shapeChecker.CheckPolygonSubType(corners);

                            Pen pen;

                            if (subType == PolygonSubType.Unknown)
                            {
                                pen = (corners.Count == 4) ? redPen : bluePen;
                            }
                            else
                            {
                                pen = (corners.Count == 4) ? brownPen : greenPen;
                            }

                            g.DrawPolygon(pen, ToPointsArray(corners));
                        }
                    }
                }

                yellowPen.Dispose();
                redPen.Dispose();
                greenPen.Dispose();
                bluePen.Dispose();
                brownPen.Dispose();
            }
            #endregion

            //check if unlockbits

            #region HoughLine //Commented
            //double houghValue = 0.5;
            //Debug.WriteLine("Starting search for lines.");
            //HoughLineTransformation lineTransform = new HoughLineTransformation();
            //// apply Hough line transofrm
            //lineTransform.ProcessImage(objectsData);
            ////Bitmap houghLineImage = lineTransform.ToBitmap();
            //// get lines using relative intensity
            //HoughLine[] lines = lineTransform.GetLinesByRelativeIntensity(houghValue);
            //Debug.WriteLine("Lines found.");
            //foreach (HoughLine line in lines)
            //{
            //    // get line's radius and theta values
            //    int r = line.Radius;
            //    double t = line.Theta;

            //    // check if line is in lower part of the image
            //    if (r < 0)
            //    {
            //        t += 180;
            //        r = -r;
            //    }

            //    // convert degrees to radians
            //    t = (t / 180) * Math.PI;

            //    // get image centers (all coordinate are measured relative
            //    // to center)
            //    int w2 = image.Width / 2;
            //    int h2 = image.Height / 2;

            //    double x0 = 0, x1 = 0, y0 = 0, y1 = 0;

            //    if (line.Theta != 0)
            //    {
            //        // none-vertical line
            //        x0 = -w2; // most left point
            //        x1 = w2;  // most right point

            //        // calculate corresponding y values
            //        y0 = (-Math.Cos(t) * x0 + r) / Math.Sin(t);
            //        y1 = (-Math.Cos(t) * x1 + r) / Math.Sin(t);
            //    }
            //    else
            //    {
            //        // vertical line
            //        x0 = line.Radius;
            //        x1 = line.Radius;

            //        y0 = h2;
            //        y1 = -h2;
            //    }
            //    Drawing.Line(objectsData,
            //        new IntPoint((int)x0 + w2, h2 - (int)y0),
            //        new IntPoint((int)x1 + w2, h2 - (int)y1),
            //        Color.Red);
            //    Debug.WriteLine("Line drawn");
            //}
            #endregion 
            
            Debug.WriteLine("Extraction ended.");
            return image;
            //return lanes;
        }

        public static Bitmap ConvertToGrayscale(Bitmap original)
        {
            //get a graphics object from the new image
            using (Graphics g = Graphics.FromImage(original))
            {

                //create the grayscale ColorMatrix
                ColorMatrix colorMatrix = new ColorMatrix(
                   new float[][] 
                    {
                    new float[] {.3f, .3f, .3f, 0, 0},
                    new float[] {.59f, .59f, .59f, 0, 0},
                    new float[] {.11f, .11f, .11f, 0, 0},
                    new float[] {0, 0, 0, 1, 0},
                    new float[] {0, 0, 0, 0, 1}
                    });

                //create some image attributes
                ImageAttributes attributes = new ImageAttributes();

                //set the color matrix attribute
                attributes.SetColorMatrix(colorMatrix);

                //draw the original image on the new image
                //using the grayscale color matrix
                g.DrawImage(original, new Rectangle(0, 0, original.Width, original.Height),
                   0, 0, original.Width, original.Height, GraphicsUnit.Pixel, attributes);

                //dispose the Graphics object
            }
            return original;
        }

        private static Point[] ToPointsArray(List<IntPoint> points)
        {
            Point[] array = new Point[points.Count];

            for (int i = 0, n = points.Count; i < n; i++)
            {
                array[i] = new Point(points[i].X, points[i].Y);
            }

            return array;
        }


        /// <summary>
        /// Converts a bitmap into an 8-bit grayscale bitmap
        /// </summary>
        public static Bitmap ColorToGrayscale(Bitmap bmp)
        {
            int w = bmp.Width,
                h = bmp.Height,
                r, ic, oc, bmpStride, outputStride, bytesPerPixel;
            PixelFormat pfIn = bmp.PixelFormat;
            ColorPalette palette;
            Bitmap output;
            BitmapData bmpData, outputData;

            //Create the new bitmap
            output = new Bitmap(w, h, PixelFormat.Format8bppIndexed);

            //Build a grayscale color Palette
            palette = output.Palette;
            for (int i = 0; i < 256; i++)
            {
                Color tmp = Color.FromArgb(255, i, i, i);
                palette.Entries[i] = Color.FromArgb(255, i, i, i);
            }
            output.Palette = palette;

            //No need to convert formats if already in 8 bit
            if (pfIn == PixelFormat.Format8bppIndexed)
            {
                output = (Bitmap)bmp.Clone();

                //Make sure the palette is a grayscale palette and not some other
                //8-bit indexed palette
                output.Palette = palette;

                return output;
            }

            //Get the number of bytes per pixel
            switch (pfIn)
            {
                case PixelFormat.Format24bppRgb: bytesPerPixel = 3; break;
                case PixelFormat.Format32bppArgb: bytesPerPixel = 4; break;
                case PixelFormat.Format32bppRgb: bytesPerPixel = 4; break;
                default: throw new InvalidOperationException("Image format not supported");
            }

            //Lock the images
            bmpData = bmp.LockBits(new Rectangle(0, 0, w, h), ImageLockMode.ReadOnly,
                                   pfIn);
            outputData = output.LockBits(new Rectangle(0, 0, w, h), ImageLockMode.WriteOnly,
                                         PixelFormat.Format8bppIndexed);
            bmpStride = bmpData.Stride;
            outputStride = outputData.Stride;

            //Traverse each pixel of the image
            unsafe
            {
                byte* bmpPtr = (byte*)bmpData.Scan0.ToPointer(),
                outputPtr = (byte*)outputData.Scan0.ToPointer();

                if (bytesPerPixel == 3)
                {
                    //Convert the pixel to it's luminance using the formula:
                    // L = .299*R + .587*G + .114*B
                    //Note that ic is the input column and oc is the output column
                    for (r = 0; r < h; r++)
                        for (ic = oc = 0; oc < w; ic += 3, ++oc)
                            outputPtr[r * outputStride + oc] = (byte)(int)
                                (0.299f * bmpPtr[r * bmpStride + ic] +
                                 0.587f * bmpPtr[r * bmpStride + ic + 1] +
                                 0.114f * bmpPtr[r * bmpStride + ic + 2]);
                }
                else //bytesPerPixel == 4
                {
                    //Convert the pixel to it's luminance using the formula:
                    // L = alpha * (.299*R + .587*G + .114*B)
                    //Note that ic is the input column and oc is the output column
                    for (r = 0; r < h; r++)
                        for (ic = oc = 0; oc < w; ic += 4, ++oc)
                            outputPtr[r * outputStride + oc] = (byte)(int)
                                ((bmpPtr[r * bmpStride + ic] / 255.0f) *
                                (0.299f * bmpPtr[r * bmpStride + ic + 1] +
                                 0.587f * bmpPtr[r * bmpStride + ic + 2] +
                                 0.114f * bmpPtr[r * bmpStride + ic + 3]));
                }
            }

            //Unlock the images
            bmp.UnlockBits(bmpData);
            output.UnlockBits(outputData);

            return output;
        }
    }
}
